
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include "planar_mapping.h"
#include <boost/thread.hpp>
#include "astar/AStar.h"
#include "robot_control/Motion.h"
#include <robot_control/CameraPose.h>
#include "parameter_server.h"
#include <pcl_ros/transforms.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

#define MAX_RANGE (4 * MAP_SCALE_FACTOR)
#define HALF_VIEW_ANGLE (3.141/6)///8
#define ANGLE_STEP (0.001)
#define TRANSFORM_TIMEOUT 0.5

#define Z_TOLERANCE 0.02
#define Z_TOLERANCE_DOWN 0.8  //0.24
#define Z_TOLERANCE_UP 0.8  //0.1
#define Z_OFFSET 0.2


#define MIN_BLOB_SIZE 10
#define RADIUS 1

#define WAYPOINT_DISTANCE 20
#define WAYPOINT_REACH_DISTANCE WAYPOINT_DISTANCE/3
#define SPIN_TIME 10

PlanarMapping::PlanarMapping(): 
	robot_(this),
	z_tolerance_(Z_TOLERANCE),
	z_tolerance_up_(Z_TOLERANCE_UP),
	z_tolerance_down_(Z_TOLERANCE_DOWN),
	z_offset_(Z_OFFSET),
	goal_target_(0,0),
	goal_path_(NULL),
	cur_waypoint_(NULL),
	astar_worker_(NULL),
	random_rotation_done_(false),
	initial_path_(true),
	robot_priority_(DEFAULT_PRIORITY),
	camera_pose_id_(0)
{
	old_velocity_ = 0;
	old_rotation_speed_ = 0;
	last_ekf_time_ = ros::Time::now();

	map_pub_ = nh_.advertise<sensor_msgs::Image>(
			"/planar_mapping/occupancy_map", 20);

	camera_pose_pub_ = nh_.advertise<robot_control::CameraPose>(
			"camera_pose", 10);

	dummy_map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(
			"map", 1, true);

	initial_pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(
			"initialpose", 1, true);

	planar_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>(
			"/rgbdslam/reframed_cloud", 20,
			&PlanarMapping::planarCallback, this);

	landmarks_sub_ = nh_.subscribe<robot_control::Landmarks>(
			"/rgbdslam/landmarks", 1000,
			&PlanarMapping::landmarksCallback, this);

	ROS_INFO("Planar mapping ready");
	camera_pose_file = fopen("camera_pose.graph","w+");

	nh_.param("/rgbdslam/use_shortest_path", use_shortest_path_, true);

	//occupancy_map_.addTrajectory(&camera_trajectory_);
	occupancy_map_.addTrajectory(&odometry_trajectory_);
	occupancy_map_.addTrajectory(&targets_fake_trajectory_);
	occupancy_map_.addTrajectory(&goal_traj_);
	occupancy_map_.addTrajectory(&landmarks_trajectory_);
	//occupancy_map_.addTrajectory(&waypoint_fake_trajectory_);


}

void PlanarMapping::planarCallback(
		const sensor_msgs::PointCloud2ConstPtr& msg){

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr original_cloud(
			new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB> flat_cloud;
	ROS_INFO("Added one Node and received point cloud %d %d", msg->point_step,
			msg->row_step);
	pcl::fromROSMsg(*msg, *original_cloud);

	tf::StampedTransform transform;
	try {
		tr_listener_.waitForTransform("/openni_camera", "/slam_transform",
				msg->header.stamp, ros::Duration(TRANSFORM_TIMEOUT));
		tr_listener_.lookupTransform("/openni_camera", "/slam_transform",
				msg->header.stamp, transform);
	} catch (...) {
		ROS_ERROR("Cannot lookup transform. Discarding pointcloud");
		return;
	}

	btScalar pitch, roll, yaw;
	btMatrix3x3(transform.getRotation()).getRPY(roll, pitch,yaw);
	double delta_roll = -M_PI/2 - roll;
	double delta_pitch = -pitch;
	tf::Transform correct_tf;
	tf::Quaternion correct_rotation;

	//Delete roll and pitch noise. We assume that kinect is
	//always parallel to ground
	correct_rotation.setRPY(delta_roll, delta_pitch, 0);
	correct_tf.setRotation(
			transform.getRotation() * correct_rotation);
	correct_tf.setOrigin(transform.getOrigin());

	btMatrix3x3(correct_tf.getRotation()).getRPY(roll, pitch, yaw);

	pcl_ros::transformPointCloud(*original_cloud, *original_cloud, correct_tf);

	pcl::PassThrough<pcl::PointXYZRGB> pass;
	pass.setInputCloud(original_cloud);
	pass.setFilterFieldName ("z");
	pass.setFilterLimits (z_offset_ - z_tolerance_down_,
			z_offset_ + z_tolerance_up_);

	pass.filter(flat_cloud);
	for (size_t i = 0; i < flat_cloud.points.size(); ++i) {
		flat_cloud.points[i].z = 0;
	}

	pclToOccupancyMap_(flat_cloud, correct_tf);

	sensor_msgs::Image::Ptr img_msg = occupancy_map_.getImgMessage();
	map_pub_.publish(img_msg);
}

void PlanarMapping::landmarksCallback(const robot_control::LandmarksConstPtr& msg){
	if (ParameterServer::instance()->get<bool>("enable_ekf") == false) {
		return;
	}
	ROS_INFO("New landmarks received");

	landmarks_trajectory_.clear();
	for (size_t i = 0; i < msg->id.size(); i++) {
		Coordinates lm_position(msg->x[i] * MAP_SCALE_FACTOR,
				msg->y[i] * MAP_SCALE_FACTOR); 
		landmarks_trajectory_.push_back(lm_position);
	}

	double x,y,rot;
	x = curr_pose_x_/MAP_SCALE_FACTOR;
	y = curr_pose_y_/MAP_SCALE_FACTOR;
	rot = curr_pose_rot_;
	if (msg->id.size()) {
		ros::Duration time_elapsed = ros::Time::now() - last_ekf_time_;
		ekf_.AssociateLandmarks(msg, x, y, rot);
		ROS_INFO("Prev pose: %.5lf,%.5lf,%.5lf",
				curr_pose_x_/MAP_SCALE_FACTOR, curr_pose_y_/MAP_SCALE_FACTOR,
				curr_pose_rot_);
		ekf_.CorrectPose(&x, &y, &rot, old_velocity_, old_rotation_speed_,
				time_elapsed.toSec());
		ROS_INFO("ekf pose:%.5lf,%.5lf,%.5lf", x, y, rot);
	}

	old_velocity_ =	curr_velocity_;
	old_rotation_speed_ =	curr_rotation_speed_;
	last_ekf_time_ = ros::Time::now();
}


void PlanarMapping::odometryCallback(double x, double y, double rot,
		double velocity, double rotation_speed, int priority){
	Coordinates odometry_position(x * MAP_SCALE_FACTOR,
			y * MAP_SCALE_FACTOR); 
	double odometry_angle = rot;
	robot_priority_ = priority;
	static int tt = 0;

	if (tt++ % 100 == 0) {
		sensor_msgs::Image::Ptr img_msg = occupancy_map_.getImgMessage();
		map_pub_.publish(img_msg);
		ROS_INFO("Send map");
	}

	curr_velocity_ = velocity;
	curr_rotation_speed_ = rotation_speed;

	nav_msgs::OccupancyGrid dummy_map;
	dummy_map.info.width = 1000;
	dummy_map.info.height = 1000;
	dummy_map.info.resolution = 1.0;
	dummy_map.info.origin.position.x = 0;
	dummy_map.info.origin.position.y = 0;
	dummy_map.info.origin.position.z = 0;
	dummy_map.info.origin.orientation.x = 0;
	dummy_map.info.origin.orientation.y = 0;
	dummy_map.info.origin.orientation.z = 0;
	dummy_map.info.origin.orientation.w = 1;
	int size = dummy_map.info.width * dummy_map.info.height;

	dummy_map.set_data_size(size);
	for (int i=0; i<size; i++) {
		dummy_map.data[i] = 0;
	}

	dummy_map_pub_.publish(dummy_map);

	geometry_msgs::PoseWithCovarianceStamped p;
	//initial_pose_pub_.publish(p);

	tf::TransformBroadcaster broadcaster;
	broadcaster.sendTransform(
			tf::StampedTransform(
				tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.0, 0.0, 0.0)),
				ros::Time::now(),"base_link", "base_laser"));

	if (odometry_trajectory_.size() == 0) {
		odometry_trajectory_.push_back(odometry_position);
	} else if (odometry_trajectory_.back() != odometry_position) {
		odometry_trajectory_.push_back(odometry_position);
	}

	bool goal_reached = false;
	if(robot_priority_ < DEFAULT_PRIORITY){
		if (goal_path_) {
			delete goal_path_;
			goal_path_ = NULL;
		}
		if (cur_waypoint_) {
			cur_waypoint_ = NULL;
		}
		goal_traj_.clear();
		// The robot is in high prioority mode. I cannot do anything.
		return;
	}

	// We compute the targets and the goal path only if we do not have one yet
	if (!goal_path_ || initial_path_ ) {
		//ROS_INFO("Recompute targets");	
		detectBlobs();
	}

	if (goal_path_ ){
		initial_path_ = false;

		// If i have a waypoint to go, I check if i reached it
		if (cur_waypoint_ != NULL) {
			//ROS_INFO("Waypoint %f,%f not yet reach", cur_waypoint_->first, cur_waypoint_->second);
			double	dist = getPointsDistance(*cur_waypoint_,
					odometry_position);

			if (dist < WAYPOINT_REACH_DISTANCE){
				ROS_INFO("Waypoint already reached");	
				StopRobot();
				cur_waypoint_ = NULL;
			}
		}

		// If I do not have a waypoint, because i reached or because i never
		// selected it, I choose on
		if (!cur_waypoint_) {
			goal_path_->clear();
			ROS_INFO("GOAL_TARGET %f,%f",goal_target_.first,goal_target_.second);
			AStar astar;
			astar.launchAstar(odometry_position, goal_target_,
					occupancy_map_, goal_path_);

			ROS_INFO("Recomputed path for same goal, pathsize %d", (int) goal_path_->size());

			// If I cannot compute the path, stop the robot and return
			if (!goal_path_) {
				StopRobot();
				ROS_INFO("Failed to recompute the path.");
				goal_traj_.clear();
				return;
			} else {
				ROS_INFO("Recomputed the path successful");
				goal_reached = selectWaypoint();
				// Copy the goal path into goal_traj_
				goal_traj_.assign(goal_path_->begin(), goal_path_->end());
				sensor_msgs::Image::Ptr img_msg = occupancy_map_.getImgMessage();
				map_pub_.publish(img_msg);
			}
		}
		//maybe never see this line

		// During the selection of the waypoint I can detect that i'm close
		// enough to the goal. In that case I declare that I arrived and I stop
		if (goal_reached) {
			ROS_INFO("goal reached");
			StopRobot();
			cur_waypoint_ = NULL;
			delete goal_path_;
			goal_path_ = NULL;
			return;
		}



		// If I'here it means that I have a waypoint and I need to reach it
		double delta_angle = getPointsDeltaAngle(*cur_waypoint_,
				odometry_position, odometry_angle); 
		if(delta_angle > M_PI || delta_angle < -M_PI){
			delta_angle = delta_angle - M_PI;
		}
		double	dist = getPointsDistance(*cur_waypoint_, odometry_position);

		ROS_INFO("dist %f, angle %f",dist, delta_angle);

		if(dist > WAYPOINT_REACH_DISTANCE){
			robot_.setMotion(dist, delta_angle);
		}else{
			ROS_INFO("Waypoint reached, dist %f",dist);	
			StopRobot();
			cur_waypoint_ = NULL;		
		}
	} else if (!random_rotation_done_ && !initial_path_){
		random_rotation_done_ = true;

		ROS_ERROR("No path found... spinning");
		ros::Duration spin_time(SPIN_TIME);
		robot_.setMotion(0, M_PI);
		spin_time.sleep();
		ROS_ERROR("Stop spinning");
		StopRobot();

	} else if(!initial_path_) {
		ROS_ERROR("Sorry, no more targets to go... quitting");
		StopRobot();
		exit(0);
	}
}


bool PlanarMapping::selectWaypoint(void) {


	int distance = 0;
	Trajectory::const_iterator it;
	Coordinates temp_waypoint;
	waypoint_fake_trajectory_.clear();

	ROS_INFO("goal_path_size = %d", (int)goal_path_->size());
	for (it = goal_path_->begin(); it != goal_path_->end() && 
			distance < WAYPOINT_DISTANCE; ++it) {
		//ROS_INFO("dis = %d",distance);
		distance++;
	}
	if (distance < WAYPOINT_DISTANCE) {
		ROS_INFO("No more waypoint, goal reached");
		return true;
	} else {
		cur_waypoint_ = &(*it);
		waypoint_fake_trajectory_.push_back(*it);
		//ROS_INFO("Has waypoint for this path, dis = %d",distance);
		return false;
	}
}


double PlanarMapping::getPointsDistance(const Coordinates &a,
		const Coordinates &b) const {

	double distance = pow(a.first - b.first, 2) + pow(a.second - b.second, 2);
	return sqrt(distance);

}

double PlanarMapping::getPointsDeltaAngle(const Coordinates &a,
		const Coordinates &b, double cur_angle) const {
	double angle = 0;
	double delta_y = b.second - a.second;
	double delta_x = b.first - a.first;

	if (delta_x == 0) {
		angle = -delta_y/abs(delta_y) * M_PI/2;
	} else {
		angle = atan(delta_y/delta_x);

		if (delta_x > 0 && delta_y <= 0) {
			angle = M_PI + angle;
		} else if (delta_y > 0  && delta_x > 0){
			angle = -M_PI + angle;
		}
	}

	double turn_angle = cur_angle - angle;
	//ROS_INFO("Turning angle %.3f (mine is: %.3f)",turn_angle * 180/PI,
	//		cur_angle * 180/PI);
	return turn_angle;
}

void PlanarMapping::StopRobot(void) const {
	robot_.setMotion(0, 0);
	ROS_INFO("STOP the robot");
}

//This function using clean than update occupancy map algorithm
void PlanarMapping::pclToOccupancyMap_(
		const pcl::PointCloud<pcl::PointXYZRGB>& pcl_map,
		const tf::Transform& transform) {
	tf::Pose pose(transform);
	double trasl_x = pose.getOrigin()[0] * MAP_SCALE_FACTOR;
	double trasl_y = pose.getOrigin()[1] * MAP_SCALE_FACTOR;
	double rotation = tf::getYaw(pose.getRotation()) + M_PI/2;
	fprintf(camera_pose_file, "VERTEX2 %d %f %f %f\n", camera_pose_id_, pose.getOrigin()[0],
			pose.getOrigin()[1], tf::getYaw(pose.getRotation()));
	camera_pose_id_++;
	camera_trajectory_.push_back(Coordinates(trasl_x, trasl_y));

	robot_control::CameraPose camera_pose;
	camera_pose.x = pose.getOrigin()[0];
	camera_pose.y = pose.getOrigin()[1];
	camera_pose.rot = rotation; 
	camera_pose_pub_.publish(camera_pose);

	curr_pose_x_ = trasl_x;
	curr_pose_y_ = trasl_y;
	curr_pose_rot_ = rotation;

	OccupancyMap new_scan_map;
	for (size_t i = 0; i < pcl_map.points.size(); i++) {
		int x = pcl_map.points[i].x * MAP_SCALE_FACTOR;
		int y = pcl_map.points[i].y * MAP_SCALE_FACTOR;

		if (pointIsInVisibleRange(x, y)) {
			new_scan_map.setXY(x, y, OCCUPIED);
		}
	}

	//scanViewRange(true);
	occupancy_map_.merge(new_scan_map);
	scanViewRange();

}

void PlanarMapping::scanViewRange(bool clear) {
	for (double angle = curr_pose_rot_ - HALF_VIEW_ANGLE;
			angle < curr_pose_rot_ + HALF_VIEW_ANGLE; angle+= ANGLE_STEP) {
		bool obstacle = false;
		double end_x = MAX_RANGE * cos(angle) + curr_pose_x_;
		double end_y = MAX_RANGE * sin(angle) + curr_pose_y_;

		double dx = end_x - curr_pose_x_;
		double dy = end_y - curr_pose_y_;

		int step_count = (int) floor(MAX_RANGE) + 2;
		dx /= (step_count - 1);
		dy /= (step_count - 1);

		for (int step = 0; step < step_count && !obstacle; step++) {
			int x = curr_pose_x_ + step * dx;
			int y = curr_pose_y_ + step * dy;

			if (!clear) {
				uint8_t occupancy = occupancy_map_.getXY(x, y);

				if(occupancy != OCCUPIED) {
					occupancy_map_.setXY(x, y, FREE);
				}
				else {
					obstacle = true;
				}
			} else {
				occupancy_map_.clearXY(x, y);
			}
		}
	}
}

bool PlanarMapping::pointIsInVisibleRange(int x, int y) {
	double point_distance = sqrt(
			pow(x - curr_pose_x_, 2) + pow(y - curr_pose_y_, 2));
	if (point_distance > MAX_RANGE) {
		return false;
	}

	return true;
}

#define TRED_MIN_PATH_LENGTH 10000
void PlanarMapping::detectBlobs(void) {
	std::vector<Blob> blobs;

	OccupancyMap cells_tmp;
	cells_tmp.merge(occupancy_map_);

	for (int x = cells_tmp.getMinX(); x <= cells_tmp.getMaxX(); x++) {
		for (int y = cells_tmp.getMinY(); y <= cells_tmp.getMaxY(); y++) {
			if (cells_tmp.getXY(x, y) == FREE
					&& (cells_tmp.getXY(x+1, y) == UNKNOWN
						|| cells_tmp.getXY(x-1, y)	== UNKNOWN
						|| cells_tmp.getXY(x, y+1)	== UNKNOWN
						|| cells_tmp.getXY(x, y-1) == UNKNOWN))	{
				cells_tmp.setXY(x, y, FRONTIER);
			}
		}
	}

	// Seeking blobs
	for (int x = cells_tmp.getMinX(); x <= cells_tmp.getMaxX(); x++) {
		for (int y = cells_tmp.getMinY(); y <= cells_tmp.getMaxY(); y++) {
			// already detected blobs will have positive marked cell
			if (cells_tmp.getXY(x, y) == FRONTIER) {
				Blob new_blob;
				walkBlob(&cells_tmp, x, y, blobs.size() + 1, &new_blob);
				blobs.push_back(new_blob);
			}
		}
	}

	Target tmp_tar;
	targets_fake_trajectory_.clear();
	targets_.clear();
	int j = 1;

	int min_path_length = TRED_MIN_PATH_LENGTH + 10;
	// Calculate center
	for (unsigned int i = 0; i < blobs.size(); ++i) {
		// Print coordinates on image, if it is large enough
		if (blobs[i].num_cell > MIN_BLOB_SIZE) {
			// Show center point
			tmp_tar.id = j++;
			tmp_tar.coordinate = blobs[i].center;
			tmp_tar.rank = blobs[i].num_cell;

			if (use_shortest_path_) {
				Trajectory *tmp_path = new Trajectory;
				tmp_tar.pathlength = computeTargetPath(tmp_tar.coordinate, tmp_path);
				if (tmp_tar.pathlength > 0) {
					if (min_path_length >TRED_MIN_PATH_LENGTH ||
							tmp_tar.pathlength < min_path_length) {
						min_path_length = tmp_tar.pathlength;
						goal_target_ = tmp_tar.coordinate;
						goal_path_ = tmp_path;
					} else {
						delete tmp_path;
					}
				}
			}
			targets_.push_front(tmp_tar);
			// The following is used for drawing the targets on the occupancy map
			targets_fake_trajectory_.push_back(tmp_tar.coordinate);
		}
	}

	if (!use_shortest_path_) {
		targets_.sort();
		//targets_.reverse();
		bool found = false;
		for (Targets::iterator it = targets_.begin();
				it != targets_.end() && !found; ++it) {
			Trajectory *tmp_path = new Trajectory;
			it->pathlength = computeTargetPath(it->coordinate, tmp_path);
			if (it->pathlength > WAYPOINT_DISTANCE) {
				found = true;
				goal_target_ = it->coordinate;
				goal_path_ = tmp_path;
			}
		}
	}
	//ROS_INFO("%d targets totally",(int)targets_.size());
}

#define MIN_PATH_LENGTH 45
int  PlanarMapping::computeTargetPath(const Coordinates &goal_target_pose,
		Trajectory *path){
	Coordinates current_pose = odometry_trajectory_.back();
	AStar astar;
	astar.launchAstar(current_pose, goal_target_pose, occupancy_map_, path);
	if(path->size() > MIN_PATH_LENGTH){
		ROS_INFO("path_size %d", (int) path->size());
		return (int) path->size();
	}
	return -1;
}

void PlanarMapping::walkBlob(OccupancyMap *cells_tmp, int x, int y,
		int newId, Blob *new_blob) {
	new_blob->id = -abs(newId); 

	new_blob->min.first = x;
	new_blob->min.second = y;
	new_blob->max.first = x;
	new_blob->max.second = y;
	new_blob->num_cell = 0;

	std::list<Coordinates> unchecked_cells;
	std::vector<Coordinates> border_cells;

	Coordinates start_cell;
	start_cell.first = x;
	start_cell.second = y;
	unchecked_cells.push_back(start_cell);
	border_cells.push_back(start_cell);
	int baricenter_x;
	int baricenter_y;

	cells_tmp->setXY(x, y, newId);

	while (unchecked_cells.size() > 0) {
		Coordinates curr_cell = unchecked_cells.front();
		unchecked_cells.pop_front();

		for (int x = ((int) curr_cell.first) - RADIUS; 
				x <= ((int) curr_cell.first) + RADIUS; ++x) {
			if (x >= cells_tmp->getMinX() && x <= cells_tmp->getMaxX()) {
				for (int y = ((int) curr_cell.second) - RADIUS; 
						y	<= ((int) curr_cell.second) + RADIUS; ++y) {
					if (y >= cells_tmp->getMinY()  && y <= cells_tmp->getMaxY()) {
						if (cells_tmp->getXY(x, y) == FRONTIER) {
							Coordinates new_neighbor;
							new_neighbor.first = x;
							new_neighbor.second = y;
							unchecked_cells.push_back(new_neighbor);
							border_cells.push_back(new_neighbor);

							cells_tmp->setXY(x, y, newId);

							new_blob->num_cell++;
							new_blob->min.first = std::min(new_blob->min.first,
									(double) x);
							new_blob->min.second = std::min(new_blob->min.second,
									(double) y);
							new_blob->max.first = std::max(new_blob->max.first,
									(double) x);
							new_blob->max.second = std::max(new_blob->max.second,
									(double) y);
						}
					}
				}
			}
		}
	}

	baricenter_x = (new_blob->max.first + new_blob->min.first) / 2;
	baricenter_y = (new_blob->max.second + new_blob->min.second) / 2;

	double best_distance = -1;
	Coordinates best_center;
	best_center.first = 0;
	best_center.second = 0;

	for (unsigned int i = 0; i < border_cells.size(); i++) {
		double cur_distance = pow(border_cells[i].first - baricenter_x, 2) + pow(
				border_cells[i].second - baricenter_y, 2);
		if (best_distance < 0 || cur_distance < best_distance) {
			best_distance = cur_distance;
			best_center = border_cells[i];
		}
	}

	new_blob->center = best_center;
	ROS_DEBUG("New blob in %.2f %.2f (baricenter %d %d)",
			best_center.first, best_center.second, baricenter_x, baricenter_y);
}

